#include "printf.h"
#include "Package.h"
#define false FALSE
#define true TRUE

module DataC{
	provides interface Data;
	uses interface LogWrite;
	uses interface LogRead;
	uses interface LEDControl;
//	uses interface ConfigStorage as Config;
//    uses interface Mount;
	
}
implementation{
	
	Package p;
	bool finished = false;

	bool busy = false;
	int caller = 0;

	
	command void Data.uint32(uint32_t data){
		printf("Value: %u \r\n", data);
	}
	
	command void Data.Test(char* test){
		printf("Test msg: %s\r\n", test);
	}
	
	command void Data.Get(){
				
		int writePos, readPos;
		
		atomic{
			while(busy);
			busy = true;
		}
		
		finished = false;
		writePos = call LogWrite.currentOffset();
		readPos = call LogRead.currentOffset();
		
		
		
		printf("Seek started\r\n");
		printfflush();
		call LogRead.seek(SEEK_BEGINNING);
		printf("Waiting to read\r\n");
		printfflush();
		
		atomic{
			while(busy);
			busy = true;
		}
		
		printf("Read Started\r\n");
		printfflush();
		
		if(writePos > readPos)
		{
			printf("Writepos > readpos \r\n");
			printfflush();
						
			for(;readPos<writePos-sizeof(Package);readPos+=sizeof(Package))
			{
				printf("Readpos: %u\r\n",readPos);
				printfflush();
				call LogRead.read(&p, sizeof(Package));
				
			}
			
			finished = true;
		}
//		else if(writePos < readPos)
//		{
//			printf("Writepos < readpos \r\n");
//			printfflush();			
//			
//			while(writePos < call LogRead.currentOffset())
//			{
//				printf("Readpos: %u\r\n",call LogRead.currentOffset());
//				printfflush();
//				call LogRead.read(&p, sizeof(Package));
//			}
//			printf("Writepos > readpos \r\n");
//			printfflush();	
//			while(writePos > call LogRead.currentOffset())
//			{
//				printf("Readpos: %u\r\n",call LogRead.currentOffset());
//				printfflush();
//				call LogRead.read(&p, sizeof(Package));
//			}
//			
			finished = true;
//		}
	}

	command void Data.Stream(Package *pack){
	  	int counter;
	  	
	  	printf("%i;",pack->id);
	  	printf("%i;",pack->rssi);
	  	
	  	for(counter = 0; counter < DATA_SIZE; counter++)
	  	{
			printf("%c", pack->data[counter]);	  		
	  	}
	  	
		printf("\r\n");
	  	
	  	printfflush();
	}

	command void Data.Store(Package *pack)
	{
		//call LogWrite.erase();
	  	
		printf("Writing to memory.\r\n");
	  	printfflush();
		if(!busy)
			if(call LogWrite.append(pack, sizeof(Package)) != SUCCESS)
			{
				printf("Append Failed\r\n");
				printfflush();
				busy = false;
			}
			call LogWrite.sync();
	}
	


	event void LogWrite.syncDone(error_t error){
		atomic busy = false;
	}

	event void LogWrite.eraseDone(error_t error){
		printf("Erase done\r\n");
		printfflush();
		atomic busy = false;
	}

	event void LogWrite.appendDone(void *buf, storage_len_t len, bool recordsLost, error_t error){
		if(error != SUCCESS)
			printf("Writing not completed.\r\n");
	  	else
	  		printf("Writing completed.\r\n");
	  		
	  	printfflush();
	  	atomic busy = false;
	}

	event void LogRead.readDone(void *buf, storage_len_t len, error_t error){
		atomic busy = false;
		printf("Read done\r\n");
		printfflush();
		if(finished)
		{
			while(busy);
			busy = true;
			call LogWrite.erase();
		}
		call Data.Stream(&p);
	}

	event void LogRead.seekDone(error_t error){
		printf("Seek done\r\n");
		printfflush();
		atomic busy = false;		
	}
}